#include "math_2d.h"

////////////////////////////////////////////////////////////////////////////////

bool Math_angleIsBetween(float a, float a1, float a2)
{
  a -= a1;
  a2 -= a1;
  
  a = Math_angleWrap360(a);
  a2 = Math_angleWrap360(a2);
  
  return a > 0.0f && a < a2;
}

////////////////////////////////////////////////////////////////////////////////

float Vector2::mag() const
{
  return sqrtf(x * x + y * y);
}

void Vector2::normalize()
{
  float rd = 1.0f / sqrtf(x * x + y * y);
  
  x *= rd;
  y *= rd;
}

float Vector2::angle() const
{
  Vector2 v = *this;
  
  v.normalize();
  
  return Math_atand2(v.y, v.x);
}

float Vector2::dot(const Vector2& v) const
{
  return x * v.x + y * v.y;
}

void Vector2::operator+=(const Vector2& v)
{
  x += v.x;
  y += v.y;
}

Vector2 operator+(const Vector2& v1, const Vector2& v2)
{
  Vector2 v;
  
  v.x = v1.x + v2.x;
  v.y = v1.y + v2.y;
  
  return v;
}

Vector2 operator-(const Vector2& v1, const Vector2& v2)
{
  Vector2 v;
  
  v.x = v1.x - v2.x;
  v.y = v1.y - v2.y;
  
  return v;
}

Vector2 operator*(float n, const Vector2& v)
{
  Vector2 vec;
  
  vec.x = n * v.x;
  vec.y = n * v.y;
  
  return vec;
}

float Vector2_dis(const Vector2& v1, const Vector2& v2)
{
  float dx = v1.x - v2.x;
  float dy = v1.y - v2.y;
  
  return sqrt(dx * dx + dy * dy);
}

void Vector2_fromAngle(Vector2* v, float a)
{
  v->x = Math_cosd(a);
  v->y = Math_sind(a);
}

////////////////////////////////////////////////////////////////////////////////

void Matrix3x2_genTranslation(Matrix3x2* m, float x, float y)
{
  m->a = 1.0f;
  m->b = 0.0f;
  m->c = x;
  m->d = 0.0f;
  m->e = 1.0f;
  m->f = y;
}

////////////////////////////////////////////////////////////////////////////////

bool Circle::contains(const Vector2& p) const
{
  float dx = p.x - pos.x;
  float dy = p.y - pos.y;
  
  return (dx * dx) + (dy * dy) <= radius * radius;
}

bool Circle::containedWithin(const Rectf& r) const
{
  if (pos.x - radius < r.min_x) return false;
  if (pos.x + radius > r.max_x) return false;
  if (pos.y - radius < r.min_y) return false;
  if (pos.y + radius > r.max_y) return false;
  
  return true;
}

bool Circle::intersect(const Circle& c) const
{
  float dx = c.pos.x - pos.x;
  float dy = c.pos.y - pos.y;
  
  return (dx * dx + dy * dy) <= ((radius + c.radius) * (radius + c.radius));
}

bool Circle::intersect(const LineSegment& l) const
{
  Vector2 cv, lv, p;
  float lmag, a;
  
  cv = pos - l.p1;
  lv = l.p2 - l.p1;
  lmag = lv.mag();
  
  a = cv.dot(lv) / (lmag * lmag);
  a = Math_clamp(a, 0.0f, 1.0f);
  
  p = l.p1 + a * lv;
  
  return contains(p);
}

////////////////////////////////////////////////////////////////////////////////

bool Arc::contains(const Vector2& p)
{
  float dx = p.x - pos.x;
  float dy = p.y - pos.y;
  
  if ((dx * dx) + (dy * dy) > radius * radius)
  {
    return false;
  }
  
  Vector2 dir = p - pos;
  
  return Math_angleIsBetween(dir.angle(), angle1, angle2);
}

bool Arc::intersect(const Circle& c)
{
  float dx = c.pos.x - pos.x;
  float dy = c.pos.y - pos.y;
  
  if ((dx * dx + dy * dy) > (radius * radius + c.radius * c.radius))
  {
    return false;
  }
  
  Vector2 n1, n2;
  Vector2 v1, v2;
  float dot1, dot2;
  
  Vector2_fromAngle(&n1, Math_angleWrap360(angle1 - 90.0f));
  Vector2_fromAngle(&n2, Math_angleWrap360(angle2 + 90.0f));
  
  dot1 = n1.dot(c.pos - pos);
  dot2 = n2.dot(c.pos - pos);
  
  return dot1 < c.radius && dot2 < c.radius;
}

////////////////////////////////////////////////////////////////////////////////

Shape2D::Shape2D(const Vector2& point)
{
  _type = SHAPE_2D_POINT;
  _point = &point;
}

Shape2D::Shape2D(const Rectf& rect)
{
  _type = SHAPE_2D_RECT;
  _rect = &rect;
}

Shape2D::Shape2D(const Circle& circle)
{
  _type = SHAPE_2D_CIRCLE;
  _circle = &circle;
}

bool Shape2D::containedWithin(const Shape2D& shape)
{
  if (_type == SHAPE_2D_CIRCLE)
  {
    if (shape._type == SHAPE_2D_RECT)
    {
      return _circle->containedWithin(*shape._rect);
    }
  }
  
  return false;
}
